/* --------------------------- (C) COPYRIGHT 2021 Fortiortech ShenZhen -----------------------------
    File Name      : MotorProtect.c
    Author         : Fortiortech  Appliction Team
	Editor         : Leo.li
    Version        : V1.0
    Date           : 2021-11-06
    Description    : This file contains XX-XX-XX function used for Motor Control.
----------------------------------------------------------------------------------------------------  
                                       All Rights Reserved
------------------------------------------------------------------------------------------------- */
#include <MyProject.h>

FaultStateType     xdata   mcFaultSource;
PWMINPUTCAL        xdata   mcPwmInput;
FaultVarible      xdata mcFaultDect;
CurrentVarible     xdata   mcCurVarible;
ProtectVarible     xdata mcProtectTime;

/*  -------------------------------------------------------------------------------------------------
    Function Name  : FaultProcess
    Description    : 保护处理函数，关闭FOC输出，同时将状态变为mcFault
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void FaultProcess(void)
{
    ClrBit(DRV_CR, FOCEN);  //FOC Disable
    MOE     = 0;			//0--->输出不使能，关闭moss
    mcState = mcFault;
}


/*  -------------------------------------------------------------------------------------------------
    Function Name  : Fault_OverUnderVoltage
    Description    : 过压欠压保护函数：程序每5ms判断一次，母线电压大于过压保护值时，计数器加一，计数器值超过20次，判断为过压保护，关闭输出;反之，计数器慢慢减
                     同理，欠压保护。
                     电机过欠压保护状态下，母线电压恢复到欠压恢复值以上，过压恢复值以下时，计数器加一，超过200次后，恢复。根据档位信息来决定恢复到哪个状态。
    Date           : 2021-04-10
    Parameter      : h_Fault: [输入/出]
    ------------------------------------------------------------------------------------------------- */
void Fault_OverUnderVoltage(void)
{
    if (mcFaultSource == FaultNoSource) //程序无其他保护下
    {
        //过压保护
        if (mcFocCtrl.mcDcbusFlt > OVER_PROTECT_VALUE)   //母线电压大于过压保护值时，计数，超过20次，判断为过压保护，关闭输出;反之，计数器慢慢减
        {
            mcFaultDect.OverVoltDetecCnt++;
            
            if (mcFaultDect.OverVoltDetecCnt > 200) //检测
            {
                mcFaultDect.OverVoltDetecCnt = 0;
                mcFaultSource             = FaultOverVoltage;
            }
        }
        else if (mcFaultDect.OverVoltDetecCnt > 0)
        {
            mcFaultDect.OverVoltDetecCnt--;
        }
        
        //欠压保护
        if (mcFocCtrl.mcDcbusFlt < UNDER_PROTECT_VALUE)
        {
            mcFaultDect.UnderVoltDetecCnt++;
            
            if (mcFaultDect.UnderVoltDetecCnt > 200) //检测
            {
                mcFaultDect.UnderVoltDetecCnt = 0;
                mcFaultSource              = FaultUnderVoltage;
            }
        }
        else if (mcFaultDect.UnderVoltDetecCnt > 0)
        {
            mcFaultDect.UnderVoltDetecCnt--;
        }
    }
    
    /*******过压欠压保护恢复*********/
    #if((VlotageRecoverEnable)&&(!StartONOFF_Enable))
    {
		if ((mcState == mcFault) && ((mcFaultSource == FaultUnderVoltage) || (mcFaultSource == FaultOverVoltage)))
		{
			if ((mcFocCtrl.mcDcbusFlt < OVER_RECOVER_VALUE) && (mcFocCtrl.mcDcbusFlt > UNDER_RECOVER_VALUE)&&(mcProtectTime.VoltRecoverTimes < VlotageRecoverTimes))
			{
				mcFaultDect.VoltRecoverCnt++;
				
				if ((mcFaultDect.VoltRecoverCnt > VlotageRecoverTime))//连续检测200ms，若正常则恢复
				{
					mcProtectTime.VoltRecoverTimes++;
					mcFaultSource           = FaultNoSource;
					mcFaultDect.VoltRecoverCnt = 0;
				}
			}
			else
			{
				mcFaultDect.VoltRecoverCnt = 0;
			}
		}
    }
	#endif
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Fault_Overcurrent
    Description    : 电机运行或者启动时，当三相中某一相最大值大于OverCurrentValue，则OverCurCnt加1。
                     连续累加3次，判断为软件过流保护。执行时间约30.4us。
    Date           : 2021-04-10
    Parameter      : h_Cur: [输入/出]
    ------------------------------------------------------------------------------------------------- */
void Fault_Overcurrent(void)
{
    if ((mcState == mcRun) || (mcState == mcStart))                       // check over current in rum and open mode
    {
        // 此部分既用于软件过流保护，又用于缺相保护
        mcCurVarible.Max_ia = FOC__IAMAX;
        mcCurVarible.Max_ib = FOC__IBMAX;
        mcCurVarible.Max_ic = FOC__ICMAX;
        mcCurVarible.Max_ia = FOC__IAMAX;
        mcCurVarible.Max_ib = FOC__IBMAX;
        mcCurVarible.Max_ic = FOC__ICMAX;
        
        if ((FOC__IAMAX >= OverSoftCurrentValue)
            || (FOC__IBMAX >= OverSoftCurrentValue)
            || (FOC__ICMAX >= OverSoftCurrentValue))
        {
            mcCurVarible.OverCurCnt++;
            
            if (mcCurVarible.OverCurCnt >= 5)
            {
                mcFaultSource     = FaultSoftOVCurrent;
                mcCurVarible.Max_ia     = 0;
                mcCurVarible.Max_ib     = 0;
                mcCurVarible.Max_ic     = 0;
                mcCurVarible.OverCurCnt = 0;
            }
        }
        else if (mcCurVarible.OverCurCnt > 0)
        {
            mcCurVarible.OverCurCnt--;
        }
    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Fault_OverCurrentRecover
    Description    : 软硬件过流保护恢复
    Date           : 2021-04-10
    Parameter      : h_Fault: [输入/出]
    ------------------------------------------------------------------------------------------------- */
void Fault_OverCurrentRecover(void)
{
    if ((mcState == mcFault) && ((mcFaultSource == FaultSoftOVCurrent)
            || (mcFaultSource == FaultHardOVCurrent)) && (mcProtectTime.CurrentPretectTimes < OverCurrentRecoverTimes))
    {
        mcFaultDect.CurrentRecoverCnt++;
        
        if (mcFaultDect.CurrentRecoverCnt >= OverCurrentRecoverTime) //200*5=1s
        {
            mcFaultDect.CurrentRecoverCnt = 0;
            mcProtectTime.CurrentPretectTimes++;
            mcFaultSource = FaultNoSource;
        }
    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Fault_Start
    Description    : 启动保护函数，电机运行状态下，电机在前5s估算转速达到堵转保护值或者5s后反电动势值太低(此方法未验证)
                     或4s内还在CtrlMode状态，即速度低于MOTOR_LOOP_RPM，程序判断为启动失败，电机停机。
                     当程序判断为启动失败后，若重启次数少于或等于5次，程序立即进入校准状态，等待重启。
    Date           : 2021-04-10
    Parameter      : h_Fault: [输入/出]
    ------------------------------------------------------------------------------------------------- */
uint8 temp=0;
void Fault_Start(void)
{
    if (mcState == mcRun)
    {
        //方法一，5s内速度大于最大速度，同时反电动势值低于一定值
        if (mcFaultDect.StartSpeedCnt <= 1000)
        {
            mcFaultDect.StartSpeedCnt++;
            
            //方法一
            if (mcFocCtrl.SpeedFlt > (mcFocCtrl.EsValue * 2))
            {
                mcFaultDect.StartSpeedAndEsCnt++;
                
                if (mcFaultDect.StartSpeedAndEsCnt >= 1000)
                {
                    mcFaultDect.StartSpeedAndEsCnt = 0;
                    mcFaultSource         = FaultStart;
					temp=mcFocCtrl.EsValue;
                    mcProtectTime.SecondStartTimes++;
                    mcProtectTime.StartFlag = 1;
                }
            }
            else
            {
                mcFaultDect.StartSpeedAndEsCnt = 0;
            }
        }
        
        //方法二
        if (mcFaultDect.StartEsCnt <= 1000) //前5s，等待1.0s后，开始判断ES，如果超过一定次数，则失败
        {
            mcFaultDect.StartEsCnt++;
            mcFaultDect.StartDelay++;
            
            if (mcFaultDect.StartDelay >= 300)               // 1.5s
            {
                mcFaultDect.StartDelay = 300;
                
                if ((mcFocCtrl.EsValue < 50)) //&&(mcFocCtrl.CtrlMode==0))
                {
                    mcFaultDect.StartESCount++;
                    
                    if (mcFaultDect.StartESCount >= 60)
                    {
                        mcFaultSource = FaultStart;
						temp=2;
                        mcProtectTime.SecondStartTimes++;
                        mcFaultDect.StartDelay     = 0;
                        mcFaultDect.StartESCount   = 0;
                        mcProtectTime.StartFlag = 2;
                    }
                }
                else
                {
                    if (mcFaultDect.StartESCount > 0)
                    {
                        mcFaultDect.StartESCount--;
                    }
                }
            }
        }
        else
        {
            mcFaultDect.StartESCount = 0;
        }
        
        //方法三，长时间在CtrlMode=0状态
        if (mcFocCtrl.CtrlMode == 0)        //
        {
            mcFaultDect.StartFocmode++;
            
            if (mcFaultDect.StartFocmode >= 1000)
            {
                mcFaultDect.StartFocmode = 0;
                mcFaultSource         = FaultStart;
								temp=3;
                mcProtectTime.SecondStartTimes++;
                mcProtectTime.StartFlag = 3;
            }
        }
    }
    
    /*******启动保护恢复*********/
    #if((!StartONOFF_Enable)&&(StartRecoverEnable))
    {
        if ((mcFaultSource == FaultStart) && (mcState == mcFault) && (mcProtectTime.SecondStartTimes <= StartProtectRestartTimes))
        {
			mcFaultDect.StartReTime ++;
			if(mcFaultDect.StartReTime > StartRecoverTime)
			{
				mcFaultSource         = FaultNoSource;
				mcFaultDect.StartReTime = 0;
			}
        }
		else
		{
			mcFaultDect.StartReTime = 0;
		}
    }
    #endif
}
/*  -------------------------------------------------------------------------------------------------
    Function Name  : Fault_Stall
    Description    : 堵转保护函数，有三种保护方式，
                   第一种，
                   第二种，电机运行状态下，延迟4s判断，估算速度绝对值超过堵转速度连续5次；
                   第三种，电机运行状态下，当U,V两相电流绝对值大于堵转电流保护值连续6次；
                   当以上三种的任何一种保护触发时，电机停机，程序判断为堵转保护；
                   当堵转保护状态下，U相采集值低于堵转恢复值时，若堵转次数小于或等于堵转重启次数8次，
                   程序延迟mcStallRecover重新启动，进行校准状态。
    Date           : 2021-04-10
    Parameter      : h_Fault: [输入/出]
    ------------------------------------------------------------------------------------------------- */
void Fault_Stall(void)
{
    if (mcState == mcRun)
    {
        if (mcFaultDect.StallDelayCnt <= 400) //5s
        {
            mcFaultDect.StallDelayCnt++;
        }
        else
        {
            //method 1，判断反电动势太小或当反电动势太小，转速太大
            if ((mcFocCtrl.EsValue < 100) || ((FOC__EOME > _Q15(500.0 / MOTOR_SPEED_BASE)) && (mcFocCtrl.EsValue < 150)))
            {
                mcFaultDect.StallDectEs++;
                
                if (mcFaultDect.StallDectEs >= 30)
                {
                    mcFaultDect.StallDectEs = 0;
                    mcFaultSource        = FaultStall;
                    mcProtectTime.StallTimes++;
                    mcProtectTime.StallFlag = 1;
                }
            }
            else
            {
                if ( mcFaultDect.StallDectEs > 0)
                {
                    mcFaultDect.StallDectEs--;
                }
            }
            
            //method 2，判断速度低于堵转最小值或者超过堵转最大值
            if ((mcFocCtrl.SpeedFlt < Motor_Stall_Min_Speed) || (mcFocCtrl.SpeedFlt > Motor_Stall_Max_Speed))
            {
                mcFaultDect.StallDectSpeed++;
                
                if (mcFaultDect.StallDectSpeed >= 300)
                {
                    mcFaultDect.StallDectSpeed = 0;
                    mcFaultSource           = FaultStall;
                    mcProtectTime.StallTimes++;
                    mcProtectTime.StallFlag = 2;
                }
            }
            else
            {
                if (mcFaultDect.StallDectSpeed > 0)
                {
                    mcFaultDect.StallDectSpeed--;
                }
            }
            
        }
    }
    
    #if((!StartONOFF_Enable)&&(StallRecoverEnable))
    {
        /*******堵转保护恢复*********/
        if ((mcFaultSource == FaultStall) && (mcState == mcFault) && (mcProtectTime.StallTimes <= StallProtectRestartTimes))
        {
            mcFaultDect.StallReCount++;
            
            if (mcFaultDect.StallReCount >= StallRecoverTime)
            {
                mcFaultDect.StallReCount = 0;
                mcFaultSource         = FaultNoSource;
            }
        }
        else
        {
            mcFaultDect.StallReCount = 0;
        }
    }
    #endif
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Fault_phaseloss
    Description    : 缺相保护函数，当电机运行状态下，10ms取三相电流的最大值，
                   1.5s判断各相电流最大值，若存在两相电流值大于一定值，而第三相电流值却非常小，则判断为缺相保护，电机停机；
    Date           : 2021-04-10
    Parameter      : h_Fault: [输入/出]
    ------------------------------------------------------------------------------------------------- */
void Fault_phaseloss(void)
{
    if (mcState == mcRun)
    {
        mcFaultDect.Lphasecnt++;
        
        if (mcFaultDect.Lphasecnt > 100) //100*5=500ms
        {
            mcFaultDect.Lphasecnt = 0;
            
            if (((mcCurVarible.Max_ia > (mcCurVarible.Max_ib * 2)) || (mcCurVarible.Max_ia > (mcCurVarible.Max_ic * 2)))
                && (mcCurVarible.Max_ia > PhaseLossCurrentValue))
            {
                mcFaultDect.AOpencnt++;
            }
            else
            {
                if (mcFaultDect.AOpencnt > 0)
                {
                    mcFaultDect.AOpencnt --;
                }
            }
            
            if (((mcCurVarible.Max_ib > (mcCurVarible.Max_ia * 2)) || (mcCurVarible.Max_ib > (mcCurVarible.Max_ic * 2)))
                && (mcCurVarible.Max_ib > PhaseLossCurrentValue))
            {
                mcFaultDect.BOpencnt++;
            }
            else
            {
                if (mcFaultDect.BOpencnt > 0)
                {
                    mcFaultDect.BOpencnt --;
                }
            }
            
            if (((mcCurVarible.Max_ic > (mcCurVarible.Max_ia * 2)) || (mcCurVarible.Max_ic > (mcCurVarible.Max_ib * 2)))
                && (mcCurVarible.Max_ic > PhaseLossCurrentValue))
            {
                mcFaultDect.COpencnt++;
            }
            else
            {
                if (mcFaultDect.COpencnt > 0)
                {
                    mcFaultDect.COpencnt --;
                }
            }
            
            mcCurVarible.Max_ia = 0;
            mcCurVarible.Max_ib = 0;
            mcCurVarible.Max_ic = 0;
            SetBit(FOC_CR2, ICLR);
            
            if ((mcFaultDect.AOpencnt > 1) || (mcFaultDect.BOpencnt > 1) || (mcFaultDect.COpencnt > 1))
            {
                mcProtectTime.LossPHTimes++;
                mcFaultSource = FaultLossPhase;
            }
        }
    }
    
    #if((!StartONOFF_Enable)&&(PhaseLossRecoverEnable))
    {
        /*******缺相保护恢复*********/
        if ((mcFaultSource == FaultLossPhase) && (mcState == mcFault) && (mcProtectTime.LossPHTimes <= PhaseLossRestartTimes)) //可重启5次
        {
            mcFaultDect.mcLossPHRecCount++;
            
            if (mcFaultDect.mcLossPHRecCount >= PhaseLossRecoverTime)
            {
                mcFaultDect.AOpencnt = 0;
                mcFaultDect.BOpencnt = 0;
                mcFaultDect.COpencnt = 0;
                mcFaultSource     = FaultNoSource;
            }
        }
        else
        {
            mcFaultDect.mcLossPHRecCount = 0;
        }
    }
    #endif
}

/**
    @brief        Mos过温保护
*/
void Fault_MosTemperature(void)
{
    if (mcFocCtrl.MosTempDecFlt > OVER_Mos_Temperature)
    {
        mcFaultDect.MosTemperCnt++;
        
        if (mcFaultDect.MosTemperCnt > 500)
        {			
            mcFaultDect.MosTemperCnt     = 0;
            mcFaultDect.MosTemperRecover = 0;
            mcFaultSource = FaultMosOverTemperature;
            FaultProcess();
        }
    }
    else if (mcFaultDect.MosTemperCnt > 0)
    {
        mcFaultDect.MosTemperCnt --;
    }
}

/* -------------------------------------------------------------------------------------------------
    Function Name  : MC_Control
    Description    : 电机控制状态机-故障显示
    Date           : 2021-04-10
    Parameter      : None
------------------------------------------------------------------------------------------------- */
void ERR_LED_LOOP(uint8 err){
static uint16 delay_time =300;
static uint16 led_on_cnt =0;

	if(err==0){
		led_on_cnt=0;
		delay_time=300;
//		GP06=1;
		ResetLEDPin;
		return;
	}
	if(delay_time--){
		return;
	}
	delay_time=300;
	if(led_on_cnt<(err*2)){
//		GP06^=0;
		LEDPin^=0x01;
	}

	led_on_cnt++;
	if(led_on_cnt==26)
	led_on_cnt=0;
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Fault_Detection
    Description    : 保护函数，因保护的时间响应不会很高，采用分段处理，每5个定时器中断执行一次对应的保护
                     常见保护有过欠压、过温、堵转、启动、缺相等保护，调试时，可根据需求，一个个的调试加入。
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void Fault_Detection(void)//在一毫秒中断中执行
{
    mcFaultDect.segment++;
    
    if (mcFaultDect.segment >= 5)
    {
        mcFaultDect.segment = 0;
    }
    
    if (mcFaultDect.segment == 0)
    {
        #if (CurrentRecoverEnable) //过流保护恢复使能
        {
            Fault_OverCurrentRecover();
        }
        #endif
    }
    else if (mcFaultDect.segment == 1)
    {
        #if (VoltageProtectEnable == 1) //过压保护使能
        {
            Fault_OverUnderVoltage();
        }
        #endif
    }
    else if (mcFaultDect.segment == 2)
    {
        #if (StartProtectEnable == 1) //启动保护使能
        {
            Fault_Start();
        }
        #endif
    }
    else if (mcFaultDect.segment == 3)
    {
        #if (StallProtectEnable == 1) //堵转保护使能
        {
            Fault_Stall();
        }
        #endif
    }
    else if (mcFaultDect.segment == 4)
    {
        #if (PhaseLossProtectEnable == 1) //缺相保护使能
        {
            Fault_phaseloss();
        }
        #endif
    }
	
	#if (Mos_TemperatureProtectEnable == Enable)
    {
        Fault_MosTemperature();            // Mos温度保护
    }
    #endif
	
		ERR_LED_LOOP(mcFaultSource);
}